#include "motor.h"
#include "math.h"
#include "stm32f1xx_hal_tim.h"
#include <stdlib.h>

#define AT8236_AIN2_Pin       GPIO_PIN_12
#define AT8236_AIN2_GPIO_Port GPIOB
#define AT8236_AIN1_Pin       GPIO_PIN_13
#define AT8236_AIN1_GPIO_Port GPIOB
#define AT8236_BIN1_Pin       GPIO_PIN_14
#define AT8236_BIN1_GPIO_Port GPIOB
#define AT8236_BIN2_Pin       GPIO_PIN_15
#define AT8236_BIN2_GPIO_Port GPIOB

#define AT8236_AIN2(x)                                                                 \
    do {                                                                               \
        x ? HAL_GPIO_WritePin(AT8236_AIN2_GPIO_Port, AT8236_AIN2_Pin, GPIO_PIN_SET)    \
          : HAL_GPIO_WritePin(AT8236_AIN2_GPIO_Port, AT8236_AIN2_Pin, GPIO_PIN_RESET); \
    } while (0)
#define AT8236_AIN1(x)                                                                 \
    do {                                                                               \
        x ? HAL_GPIO_WritePin(AT8236_AIN1_GPIO_Port, AT8236_AIN1_Pin, GPIO_PIN_SET)    \
          : HAL_GPIO_WritePin(AT8236_AIN1_GPIO_Port, AT8236_AIN1_Pin, GPIO_PIN_RESET); \
    } while (0)
#define AT8236_BIN2(x)                                                                 \
    do {                                                                               \
        x ? HAL_GPIO_WritePin(AT8236_BIN2_GPIO_Port, AT8236_BIN2_Pin, GPIO_PIN_SET)    \
          : HAL_GPIO_WritePin(AT8236_BIN2_GPIO_Port, AT8236_BIN2_Pin, GPIO_PIN_RESET); \
    } while (0)
#define AT8236_BIN1(x)                                                                 \
    do {                                                                               \
        x ? HAL_GPIO_WritePin(AT8236_BIN1_GPIO_Port, AT8236_BIN1_Pin, GPIO_PIN_SET)    \
          : HAL_GPIO_WritePin(AT8236_BIN1_GPIO_Port, AT8236_BIN1_Pin, GPIO_PIN_RESET); \
    } while (0)

extern TIM_HandleTypeDef htim1;

void motor_drive(int pwm1, int pwm2)
{
    motor_driveL(pwm1);
    motor_driveR(pwm2);
}

void motor_driveL(int pwm)
{
    if (pwm > 0) {
        AT8236_AIN1(1);
        AT8236_AIN2(0);
    }
    else {
        AT8236_AIN1(0);
        AT8236_AIN2(1);
    }
    __HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_4, abs(pwm));
}
void motor_driveR(int pwm)
{
    if (pwm > 0) {
        AT8236_BIN1(1);
        AT8236_BIN2(0);
    }
    else {
        AT8236_BIN1(0);
        AT8236_BIN2(1);
    }
    __HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_1, abs(pwm));
}
